#include "module_base/subscribe_test_nodelet.h"

namespace module_base
{

void SubTestNodelet::onInit()
{
	init_nodelet();
	// subscribers_["input"]=nh_.subscribe(params_["input_topic"],
	// 	10,&TestNodelet::test_callback,this);
	// publishers_["output"]=nh_.advertise<std_msgs::String>(
	// 	params_["output_topic"],10);
	count_=0;
	total_diff_=0;
	CREATE_SUBSCRIPION("image_test",
		&SubTestNodelet::test_callback,10);


}


void SubTestNodelet::test_callback(const sensor_msgs::Image::ConstPtr& msg)
{
	count_++;
	double diff=ros::Time::now().toSec()-msg->header.stamp.toSec();
	total_diff_+=diff;
	ROS_INFO("tiem diff: %f,%d,%f\n",diff,count_,total_diff_/count_);
}

}

PLUGINLIB_EXPORT_CLASS(module_base::SubTestNodelet, nodelet::Nodelet)